#include "fuzzy_pid.h"

/**
 * 初始化模糊控制器结构体
 * @param input_num  输入变量数量（如误差e和误差变化率de为2）
 * @param output_num 输出变量数量（如ΔKp, ΔKi, ΔKd为3）
 * @return 初始化后的模糊控制器指针
 */
Fuzzy *fuzzy_init(unsigned int input_num, unsigned int output_num) {
    Fuzzy *fuzzy_struct = (Fuzzy *) malloc(sizeof(Fuzzy));
    fuzzy_struct->input_num = input_num;  // 设置输入变量数
    fuzzy_struct->output_num = output_num; // 设置输出变量数
    // 为隶属度函数类型分配内存（输入+输出变量各有一个类型）
    fuzzy_struct->mf_type = (unsigned int *) malloc((input_num + output_num) * sizeof(unsigned int));
#ifdef FUZZY_PID_RULE_BASE_DEEP_COPY
    fuzzy_struct->mf_params = (int *) malloc(4 * FUZZY_PID_QF_DEFAULT * sizeof(int));
    fuzzy_struct->rule_base = (int *) malloc(output_num * FUZZY_PID_QF_DEFAULT * FUZZY_PID_QF_DEFAULT * sizeof(int));
#endif
    fuzzy_struct->output = (float *) malloc(output_num * sizeof(float));
    return fuzzy_struct;
}
/**
 * 释放模糊控制器资源
 * @param fuzzy_struct 要释放的模糊控制器指针
 *
 * @note
 * - 依次释放成员指针mf_type、output
 * - 最后释放结构体自身内存
 * - 被delete_pid()调用时自动释放关联的模糊控制器
 */
void delete_fuzzy(Fuzzy *fuzzy_struct) {
    free(fuzzy_struct->mf_type);
    free(fuzzy_struct->output);
    free(fuzzy_struct);
}

/**
 * 模糊参数初始化
 * @param fuzzy_struct 要初始化的模糊控制器
 * @param mf_type      隶属度函数类型 (与头文件fuzzyPID.h中定义一致)
 * @param fo_type      模糊算子类型 (0-2:与运算, 3-5:或运算, >=6:平衡算子)
 * @param df_type      解模糊方法类型 (0=重心法)
 * @param mf_params    隶属度函数参数数组
 * @param rule_base    模糊规则库二维数组[output_num][FUZZY_PID_QF_DEFAULT*FUZZY_PID_QF_DEFAULT]
 *
 * 初始化流程：
 * 1. 设置所有变量的隶属度函数类型
 * 2. 初始化输出值为0
 * 3. 根据编译选项复制或引用规则库
 * 4. 设置模糊算子和解模糊类型
 */
void fuzzy_params_init(Fuzzy *fuzzy_struct, unsigned int mf_type, unsigned int fo_type, unsigned int df_type,
                       int mf_params[], int rule_base[][FUZZY_PID_QF_DEFAULT]) {
    for (unsigned int i = 0; i < fuzzy_struct->input_num + fuzzy_struct->output_num; ++i) {
        fuzzy_struct->mf_type[i] = mf_type;
    }

    for (unsigned int i = 0; i < fuzzy_struct->output_num; ++i) {
        fuzzy_struct->output[i] = 0;
    }

#ifdef FUZZY_PID_RULE_BASE_DEEP_COPY
    for (unsigned int j = 0; j < 4 * FUZZY_PID_QF_DEFAULT; ++j) {
         fuzzy_struct->mf_params[j] = mf_params[j];
     }

     for (unsigned int k = 0; k < fuzzy_struct->output_num * FUZZY_PID_QF_DEFAULT; ++k) {
         for (unsigned int i = 0; i < FUZZY_PID_QF_DEFAULT; ++i) {
             fuzzy_struct->rule_base[k * 7 + i] = rule_base[k][i];
         }
     }
#else
    fuzzy_struct->mf_params = mf_params;
    fuzzy_struct->rule_base = (int *) rule_base;
#endif

    fuzzy_struct->fo_type = fo_type;
    fuzzy_struct->df_type = df_type;
}

/** 倒数计算宏 */
#define inverse(parameter) 1.0f/(float)parameter

// Gaussian membership function
/**
 * 高斯隶属度函数计算
 * @param x     输入值
 * @param sigma 标准差，控制曲线宽度
 * @param c     均值，控制曲线中心位置
 * @return float 隶属度值 [0,1]
 */
float gaussmf(float x, float sigma, float c) {
    return expf(-powf(((x - c) / sigma), 2.0f)); // 高斯函数公式：e^(-((x-c)/σ)^2)
}

// Generalized bell-shaped membership function
/**
 * 广义钟形隶属度函数计算
 * @param x     输入值
 * @param a     宽度参数，控制曲线宽度
 * @param b     斜率参数，控制曲线陡峭度
 * @param c     中心位置参数，控制曲线中心
 * @return float 隶属度值 [0,1]
 */
float gbellmf(float x, float a, float b, float c) {
    return inverse(1.0f + powf(fabsf((x - c) / a), 2.0f * b));
}

// Sigmoidal membership function
/**
 * S形隶属度函数计算
 * @param x     输入值
 * @param a     斜率参数，控制曲线陡峭度
 * @param c     中心位置参数，控制曲线中点位置
 * @return float 隶属度值 [0,1]
 */
float sigmf(float x, float a, float c) {
    return inverse(1.0f + expf(a * (c - x)));
}

// Trapezoidal membership function
/**
 * 梯形隶属度函数计算
 * @param x     输入值
 * @param a     左底边界
 * @param b     左顶边界
 * @param c     右顶边界
 * @param d     右底边界
 * @return float 隶属度值 [0,1]
 */
float trapmf(float x, float a, float b, float c, float d) {
    if (x >= a && x < b)
        return (x - a) / (b - a);
    else if (x >= b && x < c)
        return 1.0f;
    else if (x >= c && x <= d)
        return (d - x) / (d - c);
    else return 0.0f;
}

// Triangular membership function
/**
 * 三角隶属度函数计算
 * @param x     输入值
 * @param a     左底边界
 * @param b     顶点位置
 * @param c     右底边界
 * @return float 隶属度值 [0,1]
 */
float trimf(float x, float a, float b, float c) {
    return trapmf(x, a, b, b, c);
}

// Z-shaped membership function
/**
 * Z形隶属度函数计算
 * @param x     输入值
 * @param a     左边界起始位置
 * @param b     右边界结束位置
 * @return float 隶属度值 [0,1]
 */
float zmf(float x, float a, float b) {
    if (x <= a)
        return 1.0f;
    else if (x >= a && x <= (a + b) / 2.0f)
        return 1.0f - 2.0f * powf((x - a) / (b - a), 2.0f);
    else if (x >= (a + b) / 2.0f && x < b)
        return 2.0f * powf((x - b) / (b - a), 2.0f);
    else return 0;
}

// Membership function
/**
 * 隶属度函数分发器
 * @param x         误差
 * @param mf_type   隶属度函数类型：
 *                  0-高斯函数 1-钟形函数 2-S形函数 
 *                  3-梯形函数 5-Z形函数 其他-三角函数
 * @param params    函数参数数组，不同函数需要的参数个数不同：
 *                  - 高斯函数：[标准差σ, 中心值c]
 *                  - 钟形函数：[宽度a, 斜率b, 中心值c]
 *                  - S形函数：[斜率a, 中心值c]
 *                  - 梯形函数：[左底a, 左顶b, 右顶c, 右底d]
 *                  - Z形函数：[左边界a, 右边界b]
 *                  - 三角函数：[左底a, 顶点b, 右底c]
 * @return float    隶属度值 [0,1]
 *
 * @note
 * - 根据mf_type调用对应的具体隶属度函数
 * - 参数数组必须与函数类型要求的参数个数匹配
 * - 默认使用三角隶属度函数作为fallback
 */
float mf(float e, unsigned int mf_type, int *params) {
    switch (mf_type) {
        case Gaussmf:  // 高斯隶属度函数
            return gaussmf(e, params[0], params[1]);
        case Gbellmf:  // 广义钟形隶属度函数
            return gbellmf(e, params[0], params[1], params[2]);
        case Sigmf:  // S形隶属度函数
            return sigmf(e, params[0], params[2]);
        case Trapmf:  // 梯形隶属度函数
            return trapmf(e, params[0], params[1], params[2], params[3]);
        case Zmf:  // Z形隶属度函数
            return zmf(e, params[0], params[1]);
        default: // 默认使用三角隶属度函数
            return trimf(e, params[0], params[1], params[2]);
    }
}

// 并处理
/**
 * 模糊逻辑或运算
 * @param a     第一个隶属度值
 * @param b     第二个隶属度值
 * @param type  运算类型：
 *              1-代数和 2-有界和 其他-取最大值
 * @return float 运算结果 [0,1]
 */
float or(float a, float b, unsigned int type) {
    if (type == 1) { // algebraic sum
        return a + b - a * b;
    } else if (type == 2) { // bounded sum
        return fminf(1, a + b);
    } else { // fuzzy union
        return fmaxf(a, b);
    }
}

/**
 * 模糊逻辑与运算
 * @param a     第一个隶属度值
 * @param b     第二个隶属度值
 * @param type  运算类型：
 *              1-代数积 2-有界积 其他-取最小值
 * @return float 运算结果 [0,1]
 */
float and(float a, float b, unsigned int type) {
    if (type == 1) { // algebraic product
        return a * b;
    } else if (type == 2) { // 有界积
        return fmaxf(0, a + b - 1);
    } else { // fuzzy intersection
        return fminf(a, b);
    }
}

/**
 * 平衡算子计算
 * @param a       第一个隶属度值
 * @param b       第二个隶属度值
 * @param params  平衡参数 [0,1]：
 *                0-完全使用与运算，1-完全使用或运算
 * @return float  平衡后的运算结果 [0,1]
 */
float equilibrium(float a, float b, float params) {
    return powf(a * b, 1 - params) * powf(1 - (1 - a) * (1 - b), params);
}

/**
 * 模糊运算符选择器
 * @param a     第一个隶属度值
 * @param b     第二个隶属度值
 * @param type  运算符类型：
 *              0-2: 与运算（取小/代数积/有界积）
 *              3-5: 或运算（取大/代数和/有界和）
 *              >=6: 平衡算子（默认参数0.5）
 * @return float 运算结果 [0,1]
 */
float fo(float a, float b, unsigned int type) {
    if (type < 3) {
        return and(a, b, type);
    } else if (type < 6) {
        return or(a, b, type - 3);
    } else {
        return equilibrium(a, b, 0.5f);
    }
}

/**
 * 重心法解模糊计算
 * @param joint_membership 联合隶属度矩阵（二维数组扁平化存储）
 * @param index            有效隶属度索引数组
 * @param count            各输入的有效隶属度数量 [e_count, de_count]
 * @param fuzzy_struct     模糊控制器结构体指针
 * 
 * 实现说明：
 * 1. 计算所有规则激活强度的总和（分母项）
 * 2. 对每个输出变量计算加权和（分子项）
 * 3. 最终输出值 = 分子项 / 分母项
 * 4. 支持多输出并行计算，结果存储于fuzzy_struct->output[]
 */
void moc(const float *joint_membership, const unsigned int *index, const unsigned int *count, Fuzzy *fuzzy_struct) {

    // 总和
    float denominator_count = 0;
    // 输出值
    float numerator_count[fuzzy_struct->output_num];
    for (unsigned int l = 0; l < fuzzy_struct->output_num; ++l) {
        numerator_count[l] = 0;
    }

    // 求总和
    for (int i = 0; i < count[0]; ++i) {
        for (int j = 0; j < count[1]; ++j) {
            denominator_count += joint_membership[i * count[1] + j];
        }
    }

    for (unsigned int k = 0; k < fuzzy_struct->output_num; ++k) {
        for (unsigned int i = 0; i < count[0]; ++i) {
            for (unsigned int j = 0; j < count[1]; ++j) {
                numerator_count[k] += joint_membership[i * count[1] + j] *  // 当前计算模糊算子值 * 对应的规则库值
                                      fuzzy_struct->rule_base[k * FUZZY_PID_QF_DEFAULT * FUZZY_PID_QF_DEFAULT +  //基数 k p i
                                                              index[i] * FUZZY_PID_QF_DEFAULT +                  //第几排
                                                              index[count[0] + j]];                              //第几个
            }
        }
    }


#ifdef FUZZY_PID_DEBUG_PRINT
    // printf("\noutput:\n");
    printf("denominator_count:%f \n",denominator_count);
#endif

    for (unsigned int l = 0; l < fuzzy_struct->output_num; ++l) {
        fuzzy_struct->output[l] = numerator_count[l] / denominator_count;
        // printf("%f,%f,%f\n", numerator_count[l], denominator_count, fuzzy_struct->index[l]);
        
        #ifdef FUZZY_PID_DEBUG_PRINT
            printf("%f,%f\n", numerator_count[l], fuzzy_struct->output[l]);
        #endif

    }

/*
*/
}

/**
 * 解模糊方法选择器
 * @param joint_membership 联合隶属度矩阵
 * @param output           输出索引数组（已弃用）
 * @param count            各输入有效隶属度数量
 * @param fuzzy_struct     模糊控制器结构体
 * @param df_type          解模糊类型：
 *                        0-重心法（默认） 
 *                        其他-保留为未来扩展
 */
void df(const float *joint_membership, const unsigned int *output, const unsigned int *count, Fuzzy *fuzzy_struct,
        int df_type) {
    if(df_type == 0)
        moc(joint_membership, output, count, fuzzy_struct);
    else {
        printf("Waring: No such of defuzzifier!\n");
        moc(joint_membership, output, count, fuzzy_struct);
    }
}

/**
 * 模糊控制核心处理流程
 * @param e            标准化后的误差
 * @param de           标准化后的误差变化率
 * @param fuzzy_struct 模糊控制器实例
 * 
 * 处理流程：
 * 1. 计算输入变量的有效隶属度
 * 2. 生成联合隶属度矩阵
 * 3. 应用模糊规则库
 * 4. 解模糊得到最终输出
 */
void fuzzy_control(float e, float de, Fuzzy *fuzzy_struct) {
    // 隶属度数据和索引
    float membership[FUZZY_PID_QF_DEFAULT * 2];
    unsigned int index[FUZZY_PID_QF_DEFAULT * 2];
    // 记录误差和误差变化的 数量
    unsigned int count[2] = {0, 0};

    {   // 初始化隶属度计算
        int j = 0;
        // 遍历所有量化因子，计算误差e的隶属度
        for (int i = 0; i < FUZZY_PID_QF_DEFAULT; ++i) {
            // 输入
            float temp = mf(e, fuzzy_struct->mf_type[0], fuzzy_struct->mf_params + 4 * i);
            
            // 仅保留有效隶属度值（大于0.0001）
            if (temp > 1e-4) {
                membership[j] = temp;
                index[j++] = i;  // 记录有效隶属度的索引
            }
        }

        count[0] = j;

        // 处理误差变化率de的隶属度计算
        for (int i = 0; i < FUZZY_PID_QF_DEFAULT; ++i) {
            float temp = mf(de, fuzzy_struct->mf_type[1], fuzzy_struct->mf_params + 4 * i);
            // 过滤无效隶属度并记录索引
            if (temp > 1e-4) {
                membership[j] = temp;
                index[j++] = i;  // 同时记录de的有效隶属度索引
            }
        }

        count[1] = j - count[0];
    }

#ifdef FUZZY_PID_DEBUG_PRINT
    // printf("\nFUZZY_PID_DEBUG_PRINT\n");

    // printf("\n");
    printf("e:%f,de:%f, ",e,de);
    printf("index: ");
    for (unsigned int k = 0; k < count[0]+count[1]; ++k) {
        if(k == count[0]){
            printf("_ ");
        }
        printf("%d ", index[k]);
    }
    // printf("\n");

    printf(",count: ");
    for (unsigned int k = 0; k < 2; ++k) {
        printf("%d ", count[k]);
    }
    // printf("\n");

    printf(",membership: ");
    for (unsigned int k = 0; k < count[0]+count[1]; ++k) {
        printf("%f ", membership[k]);
    }
    
#endif

    if (count[0] == 0 || count[1] == 0) {
        for (unsigned int l = 0; l < fuzzy_struct->output_num; ++l) {
            fuzzy_struct->output[l] = 0;
        }
        return;
    }


#ifdef FUZZY_PID_DEBUG_PRINT
    printf("\njoint_membership\n");

#endif

    // Joint membership
    float joint_membership[count[0] * count[1]];

    // 计算联合隶属度（e和de的组合）
    for (int i = 0; i < count[0]; ++i) {          // 遍历e的有效隶属度索引
        for (int j = 0; j < count[1]; ++j) {      // 遍历de的有效隶属度索引
            // 使用模糊运算符组合e和de的隶属度值（类型：%d）
            joint_membership[i * count[1] + j] = fo(
                membership[i],                    // e的隶属度值 
                membership[count[0] + j],         // de的隶属度值
                fuzzy_struct->fo_type             // 模糊运算符类型
            );
            #ifdef FUZZY_PID_DEBUG_PRINT
                printf("%d:%f ",i * count[1] + j,joint_membership[i * count[1] + j]);
            #endif

        }
        #ifdef FUZZY_PID_DEBUG_PRINT
            printf("\n");
        #endif

    }

    df(joint_membership, index, count, fuzzy_struct, 0);
}

/**
 * 原始模糊PID控制器初始化（全参数版）
 * @param kp        比例系数基础值
 * @param ki        积分系数基础值
 * @param kd        微分系数基础值
 * @param integral_limit 积分限幅值
 * @param dead_zone      死区阈值
 * @param feed_forward   前馈系数
 * @param error_max      最大误差范围
 * @param delta_error_max 最大误差变化率
 * @param delta_kp_max   Kp最大调整幅度
 * @param delta_ki_max   Ki最大调整幅度
 * @param delta_kd_max   Kd最大调整幅度
 * @param mf_type       隶属度函数类型
 * @param fo_type       模糊算子类型
 * @param df_type       解模糊类型
 * @param mf_params     隶属度函数参数数组
 * @param rule_base     模糊规则库二维数组[output_num][FUZZY_PID_QF_DEFAULT*FUZZY_PID_QF_DEFAULT]
 * @param output_min_value 最小输出值
 * @param output_middle_value 中间基准值
 * @param output_max_value 最大输出值
 * @return PID控制器指针
 *
 * 注意：
 * - 该函数会创建并初始化关联的模糊控制器
 * - 规则库内存管理依赖FUZZY_PID_RULE_BASE_DEEP_COPY宏定义
 * - 输出值范围用于PWM等执行器限制
 */
struct PID *raw_fuzzy_pid_init(float kp, float ki, float kd, float integral_limit, float dead_zone,
                               float feed_forward, float error_max, float delta_error_max, float delta_kp_max,
                               float delta_ki_max, float delta_kd_max, unsigned int mf_type, unsigned int fo_type,
                               unsigned int df_type, int mf_params[], int rule_base[][FUZZY_PID_QF_DEFAULT],
                               int output_min_value, int output_middle_value, int output_max_value) {
    struct PID *pid = (struct PID *) malloc(sizeof(struct PID));
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;

    pid->delta_kp_max = delta_kp_max;
    pid->delta_ki_max = delta_ki_max;
    pid->delta_kd_max = delta_kd_max;

    pid->delta_kp = 0;
    pid->delta_ki = 0;
    pid->delta_kd = 0;

    pid->error_max = error_max;
    pid->delta_error_max = delta_error_max;

    // 根据数据大小控制是否要输出 i和d
    int output_count = 1;
    if (ki > 1e-4) {
        output_count += 1;
        if (kd > 1e-4)
            output_count += 1;
    }

    pid->fuzzy_struct = fuzzy_init(2, output_count);
    fuzzy_params_init(pid->fuzzy_struct, mf_type, fo_type, df_type, mf_params, rule_base);

    pid->last_error = 0;
    pid->current_error = 0;

    pid->intergral = 0;
    pid->intergral_limit = integral_limit;

    pid->dead_zone = dead_zone;
    pid->feed_forward = feed_forward;

    pid->output_max_value = output_max_value;
    pid->output_middle_value = output_middle_value;
    pid->output_min_value = output_min_value;

    return pid;
}


struct PID *fuzzy_pid_init(float *params, float delta_k, unsigned int mf_type, unsigned int fo_type,
                           unsigned int df_type, int mf_params[], int rule_base[][FUZZY_PID_QF_DEFAULT]) {
    return raw_fuzzy_pid_init(params[0], params[1], params[2], params[3], params[4], params[5], MAX_ERROR,
                              MAX_DELTA_ERROR, params[0] / delta_k, params[1] / delta_k, params[2] / delta_k, mf_type,
                              fo_type, df_type, mf_params,
                              rule_base, FUZZY_PID_MIN_PWM_OUTPUT, FUZZY_PID_MIDDLE_PWM_OUTPUT, FUZZY_PID_MAX_PWM_OUTPUT);
}


struct PID *fuzzy_pid_initParams(FuzzyPIDParams *params, float delta_k, unsigned int mf_type, unsigned int fo_type,
                           unsigned int df_type, int mf_params[], int rule_base[][FUZZY_PID_QF_DEFAULT]) {
    return raw_fuzzy_pid_init(params->Kp, params->Ki, params->Kd, params->integral_limit, params->dead_zone, params->feed_forward, MAX_ERROR,
                              MAX_DELTA_ERROR, params->Kp / delta_k, params->Ki / delta_k, params->Kd / delta_k, mf_type,
                              fo_type, df_type, mf_params,
                              rule_base, FUZZY_PID_MIN_PWM_OUTPUT, FUZZY_PID_MIDDLE_PWM_OUTPUT, FUZZY_PID_MAX_PWM_OUTPUT);
}

/**
 * 原始PID控制器初始化（全参数版）
 * @param kp        比例系数
 * @param ki        积分系数
 * @param kd        微分系数
 * @param integral_limit 积分限幅值
 * @param dead_zone      死区阈值
 * @param feed_forward   前馈系数
 * @param linear_adaptive_kp 线性自适应比例系数
 * @param error_max      最大误差范围
 * @param delta_error_max 最大误差变化率
 * @param output_min_value 最小输出值
 * @param output_middle_value 中间基准值
 * @param output_max_value 最大输出值
 * @return PID控制器指针
 *
 * @note
 * - 初始化时不关联模糊控制器
 * - 线性自适应参数用于动态调整比例系数
 * - 输出值范围用于PWM等执行器限制
 */
struct PID *raw_pid_init(float kp, float ki, float kd, float integral_limit, float dead_zone,
                         float feed_forward, float linear_adaptive_kp, float error_max, float delta_error_max,
                         int output_min_value, int output_middle_value, int output_max_value) {
    struct PID *pid = (struct PID *) malloc(sizeof(struct PID));
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;

    pid->delta_kp_max = 0;
    pid->delta_ki_max = 0;
    pid->delta_kd_max = 0;

    pid->delta_kp = 0;
    pid->delta_ki = 0;
    pid->delta_kd = 0;

    pid->error_max = error_max;
    pid->delta_error_max = delta_error_max;

    pid->fuzzy_struct = NULL;

    pid->last_error = 0;
    pid->current_error = 0;

    pid->intergral = 0;
    pid->intergral_limit = integral_limit;

    pid->dead_zone = dead_zone;
    pid->feed_forward = feed_forward;

    pid->output_max_value = output_max_value;
    pid->output_middle_value = output_middle_value;
    pid->output_min_value = output_min_value;

    pid->linear_adaptive_kp = linear_adaptive_kp;
    return pid;
}

/**
 * 简化版PID控制器初始化
 * @param params 参数数组[7]结构：
 *              [0]kp, [1]ki, [2]kd, [3]integral_limit,
 *              [4]dead_zone, [5]feed_forward, [6]linear_adaptive_kp
 * @return PID控制器指针
 *
 * @note
 * - 使用预定义的误差范围和输出范围
 * - 调用raw_pid_init()实现具体初始化
 * - 适用于快速初始化标准PID控制器
 */
struct PID *pid_init(float *params) {
    return raw_pid_init(params[0], params[1], params[2], params[3], params[4], params[5], params[6], MAX_ERROR,
                        MAX_DELTA_ERROR, FUZZY_PID_MIN_PWM_OUTPUT, FUZZY_PID_MIDDLE_PWM_OUTPUT, FUZZY_PID_MAX_PWM_OUTPUT);
}

/**
 * 四舍五入取整函数
 * @param parameter 待处理的浮点数
 * @return int 四舍五入后的整数值
 *
 * @note
 * - 实现原理：将浮点数放大10倍后取个位数判断
 * - 个位数>=5时进位，否则舍去
 * - 适用于快速整数近似计算
 */
int round_user(float parameter) {
    if ((int) (parameter * 10.0) % 10 >= 5)
        return parameter + 1;
    else
        return parameter;
}

/**
 * 数值限幅函数
 * @param value 输入值
 * @param max_limit 上限值
 * @param min_limit 下限值
 * @return int 限制后的值[min_limit, max_limit]
 */
int limit(int value, int max_limit, int min_limit) {
    if (value > max_limit)
        return max_limit;
    if (value < min_limit)
        return min_limit;
    return value;
}


/**
 * 模糊PID控制核心算法
 * @param real  实际测量值
 * @param idea  目标设定值
 * @param pid   PID控制器实例
 * @return float 计算得到的控制量
 *
 * 处理流程：
 * 1. 计算误差和误差变化率
 * 2. 死区处理（当启用时）
 * 3. 模糊化处理得到ΔKp, ΔKi, ΔKd
 * 4. 调整PID参数并计算积分项
 * 5. 计算最终控制输出
 *
 * 注意：
 * - 误差会进行归一化处理到[-3,3]区间
 * - 模糊输出会按比例映射到实际参数调整范围
 * - 积分项具有抗饱和功能（当启用时）
 */
float fuzzy_pid_control(float real, float idea, struct PID *pid) {
    /********** 误差处理 **********/
    // 更新误差记录
    pid->last_error = pid->current_error;          // 保存上次误差
    pid->current_error = idea - real;              // 计算当前误差（设定值-实际值）
    float delta_error = pid->current_error - pid->last_error; // 计算误差变化率

    /********** 死区处理 **********/
#ifdef FUZZY_PID_DEBUG_ZONE
    // 在正负范围内，误差等于0 
    if (pid->current_error < pid->dead_zone && pid->current_error > -pid->dead_zone) {
        pid->current_error = 0;
    } else {
    // 不在范围内，误差要减去死区大小
        if (pid->current_error > pid->dead_zone)
            pid->current_error = pid->current_error - pid->dead_zone;
        else {
            if (pid->current_error < -pid->dead_zone)
                pid->current_error = pid->current_error + pid->dead_zone;
        }
    }
#endif
    /********** 模糊控制调用 **********/
    // 将误差和误差变化率归一化到[-3,3]区间，匹配模糊论域
    fuzzy_control(
        pid->current_error / pid->error_max * 3.0f,   // 归一化后的误差e
        delta_error / pid->delta_error_max * 3.0f,    // 归一化后的误差变化率de
        pid->fuzzy_struct                             // 模糊控制器实例
    );

    /********** 模糊参数调整 **********/
    // 根据模糊输出调整PID参数（输出已归一化到[-1,1]）
    pid->delta_kp = pid->fuzzy_struct->output[0] / 3.0f * pid->delta_kp_max + pid->kp;  // ΔKp调整
    pid->delta_ki = (pid->fuzzy_struct->output_num >= 2) ? 
                   pid->fuzzy_struct->output[1] / 3.0f * pid->delta_ki_max : 0;        // ΔKi调整
    pid->delta_kd = (pid->fuzzy_struct->output_num >= 3) ? 
                   pid->fuzzy_struct->output[2] / 3.0f * pid->delta_kd_max : 0;        // ΔKd调整

#ifdef FUZZY_PID_DEBUG_PRINT
    printf("相对值 delta_kp:%f,delta_ki:%f,delta_kd:%f\n",pid->delta_kp,pid->delta_ki,pid->delta_kd);
    printf("调整后 kp:%f,ki:%f,kd:%f\n",pid->delta_kp+pid->kp,pid->delta_ki+pid->ki,pid->delta_kd+pid->kd);
    
#endif

    /********** 积分项计算 **********/
    pid->intergral += (pid->ki + pid->delta_ki) * pid->current_error;  // 带模糊调整的积分累加
    
#ifdef FUZZY_FUZZY_INTEGRAL_LIMIT
    // 积分限幅防止饱和
    if (pid->intergral > pid->intergral_limit)
        pid->intergral = pid->intergral_limit;
    else {
        if (pid->intergral < -pid->intergral_limit)
            pid->intergral = -pid->intergral_limit;
    }
#endif

    /**********  输出计算 ************/
    // PID公式: Output = Kp*e + Ki*∫e dt + Kd*de/dt + 前馈
    pid->output = (pid->kp + pid->delta_kp) * pid->current_error +  // 比例项
                  pid->intergral +                                  // 积分项
                  (pid->kd + pid->delta_kd) * (pid->current_error - pid->last_error) + // 微分项
                  pid->feed_forward * (float) idea;                 // 前馈项
    return pid->output;
}


/**
 * 普通PID控制核心算法
 * @param real  实际测量值
 * @param idea  目标设定值
 * @param pid   PID控制器实例
 * @return float 计算得到的控制量
 *
 * 处理流程：
 * 1. 计算误差和误差变化率
 * 2. 死区处理（当启用时）
 * 3. 积分项计算与限幅
 * 4. 线性自适应比例系数调整
 * 5. 计算最终控制输出
 *
 * @note
 * - 无模糊参数调整
 * - 支持线性自适应比例系数
 * - 输出包含前馈补偿
 */
float pid_control(float real, float idea, struct PID *pid) {
    pid->last_error = pid->current_error;
    pid->current_error = idea - real;

#ifdef PID_DEBUG_ZONE
    if (pid->current_error < pid->dead_zone && pid->current_error > -pid->dead_zone) {
        pid->current_error = 0;
    } else {
        if (pid->current_error > pid->dead_zone)
            pid->current_error = pid->current_error - pid->dead_zone;
        else {
            if (pid->current_error < -pid->dead_zone)
                pid->current_error = pid->current_error + pid->dead_zone;
        }
    }
#endif

#ifdef PID_DEBUG_PRINT
    printf("kp : %f, ki : %f, kd : %f\n", pid->kp, pid->ki, pid->kd);
#endif

    pid->intergral += (pid->ki) * pid->current_error;
#ifdef FUZZY_INTEGRAL_LIMIT
    if (pid->intergral > pid->intergral_limit)
        pid->intergral = pid->intergral_limit;
    else {
        if (pid->intergral < -pid->intergral_limit)
            pid->intergral = -pid->intergral_limit;
    }
#endif

    float linear_adaptive_kp = 1;
    if (pid->linear_adaptive_kp > 1e-4)
        linear_adaptive_kp =
                (1 - pid->linear_adaptive_kp) * pid->current_error / pid->error_max + pid->linear_adaptive_kp;

    pid->output = pid->kp * linear_adaptive_kp * pid->current_error + pid->intergral +
                  (pid->kd) * (pid->current_error - pid->last_error);
    pid->output += pid->feed_forward * (float) idea;
    return pid->output;
}


/**
 * 释放单个PID控制器资源
 * @param pid 要释放的PID控制器指针
 *
 * 注意：
 * - 会递归释放关联的模糊控制器结构体
 * - 被delete_pid_vector()调用时需确保指针有效性
 * - 重复释放会导致未定义行为
 */
void delete_pid(struct PID *pid) {
    if (pid->fuzzy_struct != NULL) {
        delete_fuzzy(pid->fuzzy_struct);
    }
    free(pid);
}

/**
 * 批量释放PID控制器数组
 * @param pid_vector PID控制器指针数组
 * @param count      数组长度
 *
 * @note
 * - 遍历数组逐个调用delete_pid()
 * - 最后释放数组本身内存
 * - 需确保指针有效性及count值准确
 */
/**
 * 批量删除PID控制器数组
 * @param pid_vector PID控制器指针数组
 * @param count      数组长度
 *
 * @note
 * - 遍历数组逐个调用delete_pid()
 * - 最后释放数组本身内存
 * - 需确保指针有效性及count值准确
 */
void delete_pid_vector(struct PID **pid_vector, unsigned int count) {
    for (unsigned int i = 0; i < count; ++i) {
        delete_pid(pid_vector[i]);
    }
    free(pid_vector);
}

/**
 * 批量初始化普通PID控制器数组
 * @param params  PID参数二维数组[count][FUZZY_PID_PARAMS_COUNT]
 * @param count   要创建的控制器数量
 * @return PID控制器指针数组
 *
 * 内存管理：
 * - 返回的指针数组需要调用delete_pid_vector()释放
 * - 每个元素都是独立的PID控制器实例
 */
struct PID **pid_vector_init(float params[][FUZZY_PID_PARAMS_COUNT], unsigned int count) {
    struct PID **pid = (struct PID **) malloc(sizeof(struct PID *) * count);
    for (unsigned int i = 0; i < count; ++i) {
        pid[i] = pid_init(params[i]);
    }
    return pid;
}


struct PID **
fuzzy_pid_vector_init(float params[][FUZZY_PID_PARAMS_COUNT], float delta_k, unsigned int mf_type, unsigned int fo_type,
                      unsigned int df_type, int *mf_params, int rule_base[][FUZZY_PID_QF_DEFAULT],
                      unsigned int count) {
    // malloc 了对应数量的指针，不是开辟了地址
    struct PID **pid = (struct PID **) malloc(sizeof(struct PID *) * count);
    for (unsigned int i = 0; i < count; ++i) {
        pid[i] = fuzzy_pid_init(params[i], delta_k, mf_type, fo_type, df_type, mf_params, rule_base);
    }
    return pid;
}


/**
 * 直接控制量计算（无PID调节）
 * @param zero_value  零点基准值（如PWM中间值500）
 * @param offset_value 偏移量
 * @param direct      方向控制：
 *                   true-正向叠加（zero_value + offset）
 *                   false-反向叠加（zero_value - offset）
 * @return 计算后的控制量
 *
 * 典型应用场景：
 * - 开环控制
 * - 紧急停止
 * - 手动模式控制
 */
int direct_control(int zero_value, int offset_value, bool direct) {
    if (direct == true) {
        return zero_value + offset_value;
    } else {
        return zero_value - offset_value;
    }
}


/**
 * 模糊PID电机PWM输出
 * @param real    实际测量值
 * @param idea    目标设定值
 * @param direct  控制方向
 * @param pid     PID控制器实例
 * @return int    限幅后的PWM值
 *
 * @note
 * - 先计算模糊PID输出
 * - 叠加中间基准值
 * - 最后进行输出限幅
 */
int fuzzy_pid_motor_pwd_output(float real, float idea, bool direct, struct PID *pid) {
    return limit(direct_control(pid->output_middle_value, fuzzy_pid_control(real, idea, pid), direct),
                 pid->output_max_value, pid->output_min_value);
}

/**
 * 普通PID电机PWM输出
 * @param real    实际测量值
 * @param idea    目标设定值
 * @param direct  控制方向
 *               true-正向控制
 *               false-反向控制
 * @param pid     PID控制器实例
 * @return int    限幅后的PWM值
 *
 * @note
 * - 与模糊PID版本的区别：
 *   - 使用标准PID算法
 *   - 无参数自适应调整
 * - 典型应用：
 *   - 简单控制系统
 *   - 调试基准测试
 */
int pid_motor_pwd_output(float real, float idea, bool direct, struct PID *pid) {
    return limit(direct_control(pid->output_middle_value, pid_control(real, idea, pid), direct), pid->output_max_value,
                 pid->output_min_value);
}

